#include "drivecontrol.h"

void Drivecontrol::stopManeuver(){
	motors.setSpeed(true,0);
	mice.stopFeedback();
	state_ = stop;
}
int Drivecontrol::initDriveForward (short targetSpeed)
{
	if( useFeedback_ ){
	    targetSpeed_ = targetSpeed;
	    targetAngle_ = 0;
		state_ = forward;
		mice.setupFeedback( rotationTrigger_, ( void(*)(float) ) &Drivecontrol::feedback );
	}
	state_ = feedbackless;
	motors.setSpeed(true,targetSpeed);

    return 0;
}
int Drivecontrol::initDriveBackward (short targetSpeed)
{
	if( useFeedback_ ){
		targetSpeed_ = targetSpeed;
		targetAngle_ = 0;
		state_ = backward;
		mice.setupFeedback( rotationTrigger_, ( void(*)(float) ) &Drivecontrol::feedback );
	}
	state_ = feedbackless;
	motors.setSpeed(false,targetSpeed);

    return 0;
}
int Drivecontrol::initRotate( float targetAngle, bool awaitEnd )
{
	if( useFeedback_){
	    targetSpeed_ = 0;
	    totalAngle_ = 0;
	    targetAngle_ = targetAngle;
		if(targetAngle > 0 ){
			state_ = rotateL;
			motors.rotate(false, rotationSpeed_);
		}else{
			state_ = rotateR;
			motors.rotate(true, rotationSpeed_);
		}
		mice.setupFeedback( rotationTrigger_, ( void(*)(float) ) &Drivecontrol::feedback );

		if(awaitEnd)
	    {
			log->stuur("initRotate: start awaitEnd");
			struct timespec rec, rem;

	    	//we have set the state to rotate, when rotation is finished it is set to stop
			//maybe state should be mutexed?
			while( state_ != stop ){
				//a small sleep statement so that other threads can be executed while we wait
				rec = rec_;
				nanosleep(&rec,&rem);
			}
			log->stuur("initRotate: stop awaitEnd");
	    }

	}else{
		log->stuur("rotation cannot be done without feedback");
	}
    return 0;
}
void Drivecontrol::feedback( float angle )
{

    switch( state_ )
    {
    case feedbackless:
    		log->stuur("feedback called while state == feedbackless");
    		break;
    case stop:
		log->stuur("feedback called while state == stop. Feedback has been stopped from now");
		mice.stopFeedback();
		break;
    case forward:
    case backward:
    	driveCorr(angle);
        break;
    case rotateR:
        rr( angle );
        break;
    case rotateL:
        rl( angle );
        break;
    default:
		log->stuur("feedback called while state is default");
        break;
    }
}
void Drivecontrol::driveCorr( float angle )
{
//This function get called by every significant deviation, where we adjust
//the speed of each individual motor. For the moment i assume a positive
//angle is counter clock wise
	while( abs(angle) > 0.0){
		if( angle> 0.0 ) {
			speedR_ =+ corrFactor_;
			if(speedR_ > 100 ) speedR_ = 100;
			speedL_ =- corrFactor_;
			if(speedL_ < 1 ) speedL_ = 1;
		}else{
			speedR_ =- corrFactor_;
			if(speedR_ < 1 ) speedR_ = 1;
			speedL_ =+ corrFactor_;
			if(speedL_ > 100 ) speedL_ = 100;
		}
		//we could get a deviation that is several times bigger than the basic deviation we trigger for
		//therefore we assume we have to do several adjustments
		if( angle > 0) {
			angle =- rotationTrigger_;
		}else{
			angle =+ rotationTrigger_;
		}

		//now adjust motor speeds
		if(state_ == forward){
			motors.drive_individual(true,speedL_,speedR_);
		}
		if(state_ == backward){
			motors.drive_individual(false,speedL_,speedR_);
		}

	}
}
void Drivecontrol::rr ( float angle )
{
	totalAngle_ =+ angle;
	if(totalAngle_ >= targetAngle_) {
		motors.rotate(true,0);	//we are there, stop motors
		mice.stopFeedback();
		state_ = stop;
	}
}

void Drivecontrol::rl ( float angle )
{
	totalAngle_ =+ angle;
	if(totalAngle_ >= targetAngle_) {
		motors.rotate(false,0);	//we are there, stop motors
		mice.stopFeedback();
		state_ = stop;
	}
}
Drivecontrol::Drivecontrol( bool useFeedback )
{
	log = new Log("Drivecontrol");

	//default values
	useFeedback_ = useFeedback;
    targetSpeed_ =0;
    targetAngle_ =0;
    totalAngle_ = 0;
    speed_ =0;
    corrFactor_ = 1;
    rotationTrigger_ = 0.1;
    rec_.tv_sec = 0;
    rec_.tv_nsec = 50000000;	// 1/20 second = 50*10^6 nsec
    rotationSpeed_ = 20;
    rotationDelay_.tv_sec = 0;
    rotationDelay_.tv_nsec = 100000000;		// 1/10 second = 100 * 10^6 nsec

    if( useFeedback == false) {
    	state_ = feedbackless;
    }else{
        state_ = stop;
    }
}

Drivecontrol::~Drivecontrol()
{
	delete log;
}
int Drivecontrol::testDrive( short speed, short seconds )
{

    //setup time delay
    struct timespec req, rem;
    req.tv_sec = seconds;
    req.tv_nsec =0;

    motors.setSpeed(true,speed);
    nanosleep(&req, &rem);
    motors.setSpeed(true,0);
    return 0;
}
int Drivecontrol::testRotation( short speed, short seconds ){

   //setup time delay
    struct timespec req, rem;
    req.tv_sec = seconds;
    req.tv_nsec =0;

    motors.setSpeed(false,speed);
    nanosleep(&req, &rem);
    motors.setSpeed(false,0);
    return 0;
}


